#include <math.h>
#include <string.h>
#include "pm_std.h"
#include "sm_std.h"
#include "ne_std.h"
#include "ne_dae.h"
#include "sm_ssci_run_time_errors.h"
#include "sm_RuntimeDerivedValuesBundle.h"
#include "openManipulatorTrajectoryPlanning_6a081443_1_geometries.h"
PmfMessageId openManipulatorTrajectoryPlanning_6a081443_1_compDerivs ( const
RuntimeDerivedValuesBundle * rtdv , const int * eqnEnableFlags , const double
* state , const int * modeVector , const double * input , const double *
inputDot , const double * inputDdot , const double * discreteState , double *
deriv , double * errorResult , NeuDiagnosticManager * neDiagMgr ) { const
double * rtdvd = rtdv -> mDoubles . mValues ; const int * rtdvi = rtdv ->
mInts . mValues ; int ii [ 1 ] ; double xx [ 199 ] ; ( void ) rtdvd ; ( void
) rtdvi ; ( void ) eqnEnableFlags ; ( void ) modeVector ; ( void ) inputDot ;
( void ) inputDdot ; ( void ) discreteState ; ( void ) neDiagMgr ; xx [ 0 ] =
0.2 ; xx [ 1 ] = 4.385038631106116e-3 ; xx [ 2 ] = 0.01057602111633074 ; xx [
3 ] = 0.999934457458863 ; xx [ 4 ] = xx [ 1 ] ; xx [ 5 ] = - xx [ 2 ] ; xx [
6 ] = xx [ 3 ] ; xx [ 7 ] = xx [ 1 ] * state [ 1 ] ; xx [ 8 ] = - ( xx [ 2 ]
* state [ 1 ] ) ; xx [ 9 ] = xx [ 3 ] * state [ 1 ] ; xx [ 10 ] =
1.122487308813095e-6 * state [ 1 ] ; xx [ 11 ] = - ( 2.687675606235184e-6 *
state [ 1 ] ) ; xx [ 12 ] = 1.885591678654433e-5 * state [ 1 ] ;
pm_math_Vector3_cross_ra ( xx + 7 , xx + 10 , xx + 13 ) ; xx [ 16 ] = -
0.9984344664497182 ; xx [ 17 ] = - 2.745981335367077e-3 ; xx [ 18 ] = -
0.05574004796882191 ; xx [ 19 ] = 3.758037281069619e-3 ; xx [ 10 ] = 0.5 ; xx
[ 11 ] = xx [ 10 ] * state [ 2 ] ; xx [ 12 ] = 0.01415235514121426 ; xx [ 20
] = sin ( xx [ 11 ] ) ; xx [ 21 ] = 0.9998919020591995 ; xx [ 22 ] =
3.986858461632603e-3 ; xx [ 23 ] = cos ( xx [ 11 ] ) ; xx [ 24 ] = - ( xx [
12 ] * xx [ 20 ] ) ; xx [ 25 ] = xx [ 21 ] * xx [ 20 ] ; xx [ 26 ] = xx [ 22
] * xx [ 20 ] ; pm_math_Quaternion_compose_ra ( xx + 16 , xx + 23 , xx + 27 )
; pm_math_Quaternion_inverseXform_ra ( xx + 27 , xx + 7 , xx + 16 ) ; xx [ 11
] = xx [ 12 ] * state [ 3 ] ; xx [ 19 ] = xx [ 16 ] - xx [ 11 ] ; xx [ 20 ] =
xx [ 21 ] * state [ 3 ] ; xx [ 23 ] = xx [ 17 ] + xx [ 20 ] ; xx [ 24 ] = xx
[ 22 ] * state [ 3 ] ; xx [ 25 ] = xx [ 18 ] + xx [ 24 ] ; xx [ 31 ] = xx [
19 ] ; xx [ 32 ] = xx [ 23 ] ; xx [ 33 ] = xx [ 25 ] ; xx [ 26 ] =
1.782974915729205e-3 ; xx [ 34 ] = 1.790292518937343e-3 ; xx [ 35 ] =
5.529481718271925e-5 ; xx [ 36 ] = xx [ 19 ] * xx [ 26 ] ; xx [ 37 ] = xx [
23 ] * xx [ 34 ] ; xx [ 38 ] = xx [ 25 ] * xx [ 35 ] ;
pm_math_Vector3_cross_ra ( xx + 31 , xx + 36 , xx + 39 ) ; xx [ 42 ] = -
0.9984606559510578 ; xx [ 43 ] = - 1.525595350473025e-3 ; xx [ 44 ] =
0.05467563387767577 ; xx [ 45 ] = - 9.195984813180454e-3 ; xx [ 19 ] = xx [
10 ] * state [ 4 ] ; xx [ 23 ] = 4.564927305666077e-3 ; xx [ 25 ] = sin ( xx
[ 19 ] ) ; xx [ 36 ] = 0.9999885204623049 ; xx [ 37 ] = 1.456153255698589e-3
; xx [ 46 ] = cos ( xx [ 19 ] ) ; xx [ 47 ] = xx [ 23 ] * xx [ 25 ] ; xx [ 48
] = xx [ 36 ] * xx [ 25 ] ; xx [ 49 ] = xx [ 37 ] * xx [ 25 ] ;
pm_math_Quaternion_compose_ra ( xx + 42 , xx + 46 , xx + 50 ) ;
pm_math_Quaternion_inverseXform_ra ( xx + 50 , xx + 31 , xx + 42 ) ; xx [ 19
] = xx [ 23 ] * state [ 5 ] ; xx [ 25 ] = xx [ 42 ] + xx [ 19 ] ; xx [ 38 ] =
xx [ 36 ] * state [ 5 ] ; xx [ 45 ] = xx [ 43 ] + xx [ 38 ] ; xx [ 46 ] = xx
[ 37 ] * state [ 5 ] ; xx [ 47 ] = xx [ 44 ] + xx [ 46 ] ; xx [ 54 ] = xx [
25 ] ; xx [ 55 ] = xx [ 45 ] ; xx [ 56 ] = xx [ 47 ] ; xx [ 48 ] =
3.064698286834481e-5 ; xx [ 49 ] = 1.339420127444153e-3 ; xx [ 57 ] =
1.348660701873222e-3 ; xx [ 58 ] = xx [ 25 ] * xx [ 48 ] ; xx [ 59 ] = xx [
45 ] * xx [ 49 ] ; xx [ 60 ] = xx [ 47 ] * xx [ 57 ] ;
pm_math_Vector3_cross_ra ( xx + 54 , xx + 58 , xx + 61 ) ; xx [ 64 ] = -
0.9953936295809525 ; xx [ 65 ] = - 5.058992580434195e-4 ; xx [ 66 ] =
0.09584258966610114 ; xx [ 67 ] = 2.337576029516223e-3 ; xx [ 25 ] = xx [ 10
] * state [ 6 ] ; xx [ 45 ] = 8.310758752905212e-6 ; xx [ 47 ] = sin ( xx [
25 ] ) ; xx [ 58 ] = 0.9999999999652471 ; xx [ 59 ] = 6.610584974975353e-7 ;
xx [ 68 ] = cos ( xx [ 25 ] ) ; xx [ 69 ] = xx [ 45 ] * xx [ 47 ] ; xx [ 70 ]
= xx [ 58 ] * xx [ 47 ] ; xx [ 71 ] = - ( xx [ 59 ] * xx [ 47 ] ) ;
pm_math_Quaternion_compose_ra ( xx + 64 , xx + 68 , xx + 72 ) ;
pm_math_Quaternion_inverseXform_ra ( xx + 72 , xx + 54 , xx + 64 ) ; xx [ 25
] = xx [ 45 ] * state [ 7 ] ; xx [ 47 ] = xx [ 64 ] + xx [ 25 ] ; xx [ 60 ] =
xx [ 58 ] * state [ 7 ] ; xx [ 67 ] = xx [ 65 ] + xx [ 60 ] ; xx [ 68 ] = xx
[ 59 ] * state [ 7 ] ; xx [ 69 ] = xx [ 66 ] - xx [ 68 ] ; xx [ 76 ] = xx [
47 ] ; xx [ 77 ] = xx [ 67 ] ; xx [ 78 ] = xx [ 69 ] ; xx [ 79 ] = xx [ 47 ]
* 8.094917776298291e-5 ; xx [ 80 ] = xx [ 67 ] * 3.67360393682157e-4 ; xx [
81 ] = xx [ 69 ] * 3.844288509185152e-4 ; pm_math_Vector3_cross_ra ( xx + 76
, xx + 79 , xx + 69 ) ; xx [ 47 ] = 0.05259203501857801 ; xx [ 67 ] =
4.439189670580984e-7 ; xx [ 79 ] = 0.01034590993618254 ; xx [ 80 ] = xx [ 47
] ; xx [ 81 ] = - xx [ 67 ] ; xx [ 82 ] = - xx [ 79 ] ; xx [ 83 ] =
0.7037746403582225 ; xx [ 84 ] = 0.06856918658830206 ; xx [ 85 ] = xx [ 84 ]
* input [ 6 ] ; xx [ 86 ] = 0.06856329236118894 ; xx [ 87 ] =
0.7037745357569194 ; xx [ 88 ] = xx [ 87 ] * input [ 6 ] ; xx [ 89 ] = 2.0 ;
xx [ 90 ] = ( xx [ 83 ] * xx [ 85 ] - xx [ 86 ] * xx [ 88 ] ) * xx [ 89 ] ;
xx [ 91 ] = xx [ 89 ] * ( xx [ 86 ] * xx [ 85 ] + xx [ 83 ] * xx [ 88 ] ) ;
xx [ 83 ] = input [ 6 ] - ( xx [ 87 ] * xx [ 88 ] + xx [ 84 ] * xx [ 85 ] ) *
xx [ 89 ] ; xx [ 84 ] = xx [ 90 ] ; xx [ 85 ] = xx [ 91 ] ; xx [ 86 ] = xx [
83 ] ; pm_math_Vector3_cross_ra ( xx + 80 , xx + 84 , xx + 92 ) ; xx [ 84 ] =
0.7037745357569191 ; xx [ 85 ] = 0.0685632923611889 ; xx [ 86 ] = xx [ 85 ] *
input [ 8 ] ; xx [ 87 ] = 0.06856918658830211 ; xx [ 88 ] =
0.7037746403582228 ; xx [ 95 ] = xx [ 88 ] * input [ 8 ] ; xx [ 96 ] = ( xx [
84 ] * xx [ 86 ] - xx [ 87 ] * xx [ 95 ] ) * xx [ 89 ] ; xx [ 97 ] = xx [ 89
] * ( xx [ 87 ] * xx [ 86 ] + xx [ 84 ] * xx [ 95 ] ) ; xx [ 84 ] = input [ 8
] - ( xx [ 88 ] * xx [ 95 ] + xx [ 85 ] * xx [ 86 ] ) * xx [ 89 ] ; xx [ 85 ]
= xx [ 96 ] ; xx [ 86 ] = - xx [ 97 ] ; xx [ 87 ] = xx [ 84 ] ;
pm_math_Vector3_cross_ra ( xx + 80 , xx + 85 , xx + 98 ) ; xx [ 80 ] = -
0.9952875672845549 ; xx [ 81 ] = - 0.09696730578610657 ; xx [ 85 ] = xx [ 80
] ; xx [ 86 ] = - 7.396429069217447e-8 ; xx [ 87 ] = xx [ 81 ] ; xx [ 88 ] =
4.167847961547566e-6 ; pm_math_Quaternion_inverseXform_ra ( xx + 85 , xx + 76
, xx + 101 ) ; xx [ 82 ] = 1.0e-3 ; xx [ 104 ] = xx [ 82 ] * xx [ 101 ] ; xx
[ 105 ] = xx [ 82 ] * xx [ 102 ] ; xx [ 106 ] = xx [ 82 ] * xx [ 103 ] ;
pm_math_Vector3_cross_ra ( xx + 101 , xx + 104 , xx + 107 ) ;
pm_math_Quaternion_xform_ra ( xx + 85 , xx + 107 , xx + 104 ) ; xx [ 107 ] =
xx [ 47 ] + xx [ 45 ] * state [ 10 ] ; xx [ 108 ] = xx [ 58 ] * state [ 10 ]
- xx [ 67 ] ; xx [ 109 ] = - ( xx [ 79 ] + xx [ 59 ] * state [ 10 ] ) ; xx [
82 ] = 0.017 ; pm_math_Vector3_cross_ra ( xx + 76 , xx + 107 , xx + 110 ) ;
pm_math_Vector3_cross_ra ( xx + 76 , xx + 110 , xx + 113 ) ;
pm_math_Quaternion_inverseXform_ra ( xx + 85 , xx + 113 , xx + 110 ) ; xx [
95 ] = 1.414213562373095 ; xx [ 102 ] = 0.7071067811865476 ; xx [ 113 ] = xx
[ 95 ] * xx [ 102 ] * input [ 7 ] ; xx [ 114 ] = xx [ 82 ] * xx [ 111 ] - xx
[ 113 ] ; xx [ 115 ] = 5.0 ; xx [ 116 ] = ( input [ 1 ] - state [ 10 ] - xx [
115 ] * state [ 11 ] - xx [ 114 ] ) / xx [ 82 ] ; xx [ 117 ] = xx [ 82 ] * (
xx [ 110 ] - xx [ 89 ] * xx [ 103 ] * state [ 11 ] ) ; xx [ 118 ] = xx [ 114
] + xx [ 82 ] * xx [ 116 ] ; xx [ 119 ] = ( xx [ 89 ] * xx [ 101 ] * state [
11 ] + xx [ 112 ] ) * xx [ 82 ] - ( input [ 7 ] - xx [ 113 ] ) ;
pm_math_Quaternion_xform_ra ( xx + 85 , xx + 117 , xx + 110 ) ;
pm_math_Vector3_cross_ra ( xx + 107 , xx + 110 , xx + 117 ) ; xx [ 120 ] = xx
[ 80 ] ; xx [ 121 ] = - 7.396429119177483e-8 ; xx [ 122 ] = xx [ 81 ] ; xx [
123 ] = 4.167847961603077e-6 ; pm_math_Quaternion_inverseXform_ra ( xx + 120
, xx + 76 , xx + 124 ) ; xx [ 80 ] = 1.0e-6 ; xx [ 127 ] = xx [ 80 ] * xx [
124 ] ; xx [ 128 ] = xx [ 80 ] * xx [ 125 ] ; xx [ 129 ] = xx [ 80 ] * xx [
126 ] ; pm_math_Vector3_cross_ra ( xx + 124 , xx + 127 , xx + 130 ) ;
pm_math_Quaternion_xform_ra ( xx + 120 , xx + 130 , xx + 127 ) ; xx [ 130 ] =
xx [ 47 ] - 8.310758753113379e-6 * state [ 8 ] ; xx [ 131 ] = - ( xx [ 67 ] +
xx [ 58 ] * state [ 8 ] ) ; xx [ 132 ] = 6.610584966093569e-7 * state [ 8 ] -
xx [ 79 ] ; pm_math_Vector3_cross_ra ( xx + 76 , xx + 130 , xx + 79 ) ;
pm_math_Vector3_cross_ra ( xx + 76 , xx + 79 , xx + 133 ) ;
pm_math_Quaternion_inverseXform_ra ( xx + 120 , xx + 133 , xx + 76 ) ; xx [
47 ] = xx [ 95 ] * xx [ 102 ] * input [ 9 ] ; xx [ 67 ] = xx [ 47 ] + xx [ 82
] * xx [ 77 ] ; xx [ 79 ] = ( input [ 0 ] - state [ 8 ] - xx [ 115 ] * state
[ 9 ] + xx [ 67 ] ) / xx [ 82 ] ; xx [ 101 ] = ( xx [ 89 ] * xx [ 126 ] *
state [ 9 ] + xx [ 76 ] ) * xx [ 82 ] ; xx [ 102 ] = xx [ 67 ] - xx [ 82 ] *
xx [ 79 ] ; xx [ 103 ] = xx [ 82 ] * ( xx [ 78 ] - xx [ 89 ] * xx [ 124 ] *
state [ 9 ] ) - ( input [ 9 ] - xx [ 47 ] ) ; pm_math_Quaternion_xform_ra (
xx + 120 , xx + 101 , xx + 76 ) ; pm_math_Vector3_cross_ra ( xx + 130 , xx +
76 , xx + 80 ) ; xx [ 47 ] = 0.01699999999882584 ; xx [ 67 ] =
9.33970782657223e-14 ; xx [ 95 ] = 1.123799445909647e-8 ; xx [ 101 ] =
0.01699999999999258 ; xx [ 133 ] = xx [ 47 ] ; xx [ 134 ] = -
1.412828987945046e-7 ; xx [ 135 ] = xx [ 67 ] ; xx [ 136 ] = -
1.412828987945046e-7 ; xx [ 137 ] = 1.181597059566887e-12 ; xx [ 138 ] = xx [
95 ] ; xx [ 139 ] = xx [ 67 ] ; xx [ 140 ] = xx [ 95 ] ; xx [ 141 ] = xx [
101 ] ; pm_math_Matrix3x3_postCross_ra ( xx + 133 , xx + 107 , xx + 142 ) ;
pm_math_Matrix3x3_preCross_ra ( xx + 142 , xx + 107 , xx + 133 ) ; xx [ 95 ]
= 1.0e-6 ; xx [ 102 ] = - 1.412828987980302e-7 ; xx [ 151 ] = xx [ 47 ] ; xx
[ 152 ] = xx [ 102 ] ; xx [ 153 ] = xx [ 67 ] ; xx [ 154 ] = xx [ 102 ] ; xx
[ 155 ] = 1.181597059603377e-12 ; xx [ 156 ] = 1.123799444237311e-8 ; xx [
157 ] = xx [ 67 ] ; xx [ 158 ] = 1.123799444237312e-8 ; xx [ 159 ] = xx [ 101
] ; pm_math_Matrix3x3_postCross_ra ( xx + 151 , xx + 130 , xx + 160 ) ;
pm_math_Matrix3x3_preCross_ra ( xx + 160 , xx + 130 , xx + 151 ) ; xx [ 47 ]
= 1.080949177762983e-3 - xx [ 133 ] + xx [ 95 ] - xx [ 151 ] ; xx [ 67 ] =
2.740035779081904e-24 - xx [ 134 ] + 7.573064690121713e-29 - xx [ 152 ] ; xx
[ 101 ] = 2.646977960169689e-23 ; xx [ 102 ] = 5.421010862427522e-20 - xx [
135 ] + xx [ 101 ] - xx [ 153 ] ; xx [ 103 ] = 1.085674553975849e-24 - xx [
136 ] + 1.691317780793849e-27 - xx [ 154 ] ; xx [ 113 ] =
1.367360393682158e-3 - xx [ 137 ] + xx [ 95 ] - xx [ 155 ] ; xx [ 95 ] =
4.135903062765138e-25 + xx [ 138 ] + 4.03896783473158e-28 + xx [ 156 ] ; xx [
114 ] = 2.710505431213761e-20 - xx [ 139 ] + xx [ 101 ] - xx [ 157 ] ; xx [
101 ] = 2.067951531382569e-25 + xx [ 140 ] + 2.01948391736579e-28 + xx [ 158
] ; xx [ 115 ] = 1.384428850918516e-3 - xx [ 141 ] + 1.000000000000001e-6 -
xx [ 159 ] ; xx [ 133 ] = xx [ 47 ] ; xx [ 134 ] = xx [ 67 ] ; xx [ 135 ] =
xx [ 102 ] ; xx [ 136 ] = xx [ 103 ] ; xx [ 137 ] = xx [ 113 ] ; xx [ 138 ] =
- xx [ 95 ] ; xx [ 139 ] = xx [ 114 ] ; xx [ 140 ] = - xx [ 101 ] ; xx [ 141
] = xx [ 115 ] ; xx [ 124 ] = xx [ 25 ] ; xx [ 125 ] = xx [ 60 ] ; xx [ 126 ]
= - xx [ 68 ] ; pm_math_Vector3_cross_ra ( xx + 64 , xx + 124 , xx + 151 ) ;
pm_math_Matrix3x3_xform_ra ( xx + 133 , xx + 151 , xx + 64 ) ; xx [ 25 ] = xx
[ 142 ] + xx [ 160 ] ; xx [ 60 ] = xx [ 145 ] + xx [ 163 ] ; xx [ 68 ] = xx [
148 ] + xx [ 166 ] ; xx [ 124 ] = xx [ 143 ] + xx [ 161 ] ; xx [ 125 ] = xx [
146 ] + xx [ 164 ] ; xx [ 126 ] = xx [ 149 ] + xx [ 167 ] ; xx [ 142 ] = xx [
144 ] + xx [ 162 ] ; xx [ 143 ] = xx [ 147 ] + xx [ 165 ] ; xx [ 144 ] = xx [
150 ] + xx [ 168 ] ; xx [ 154 ] = - xx [ 25 ] ; xx [ 155 ] = - xx [ 60 ] ; xx
[ 156 ] = - xx [ 68 ] ; xx [ 157 ] = - xx [ 124 ] ; xx [ 158 ] = - xx [ 125 ]
; xx [ 159 ] = - xx [ 126 ] ; xx [ 160 ] = - xx [ 142 ] ; xx [ 161 ] = - xx [
143 ] ; xx [ 162 ] = - xx [ 144 ] ; xx [ 145 ] = 0.1239983924299681 ; xx [
146 ] = - 5.656415752171374e-4 ; xx [ 147 ] = - 2.805786735064276e-4 ;
pm_math_Vector3_cross_ra ( xx + 54 , xx + 145 , xx + 148 ) ;
pm_math_Vector3_cross_ra ( xx + 54 , xx + 148 , xx + 163 ) ;
pm_math_Quaternion_inverseXform_ra ( xx + 72 , xx + 163 , xx + 54 ) ;
pm_math_Matrix3x3_xform_ra ( xx + 154 , xx + 54 , xx + 148 ) ; xx [ 163 ] =
xx [ 69 ] - xx [ 92 ] - xx [ 98 ] + xx [ 104 ] + xx [ 117 ] + xx [ 127 ] + xx
[ 80 ] + xx [ 64 ] + xx [ 148 ] ; xx [ 164 ] = xx [ 45 ] ; xx [ 165 ] = xx [
58 ] ; xx [ 166 ] = - xx [ 59 ] ; xx [ 167 ] = xx [ 70 ] - xx [ 93 ] - xx [
99 ] + xx [ 105 ] + xx [ 118 ] + xx [ 128 ] + xx [ 81 ] + xx [ 65 ] + xx [
149 ] ; xx [ 64 ] = xx [ 71 ] - xx [ 94 ] - xx [ 100 ] + xx [ 106 ] + xx [
119 ] + xx [ 129 ] + xx [ 82 ] + xx [ 66 ] + xx [ 150 ] ; xx [ 69 ] = xx [
163 ] ; xx [ 70 ] = xx [ 167 ] ; xx [ 71 ] = xx [ 64 ] ;
pm_math_Matrix3x3_xform_ra ( xx + 133 , xx + 164 , xx + 80 ) ; xx [ 65 ] =
pm_math_Vector3_dot_ra ( xx + 164 , xx + 80 ) ; ii [ 0 ] =
factorSymmetricPosDef ( xx + 65 , 1 , xx + 66 ) ; if ( ii [ 0 ] != 0 ) {
return sm_ssci_recordRunTimeError (
"sm:compiler:messages:simulationErrors:DegenerateMass" ,
 "'openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/joint4' has a degenerate mass distribution on its follower side."
, neDiagMgr ) ; } xx [ 66 ] = ( input [ 5 ] - xx [ 0 ] * state [ 7 ] -
pm_math_Vector3_dot_ra ( xx + 164 , xx + 69 ) ) / xx [ 65 ] ; xx [ 69 ] = xx
[ 163 ] + xx [ 66 ] * xx [ 80 ] ; xx [ 70 ] = xx [ 167 ] + xx [ 66 ] * xx [
81 ] ; xx [ 71 ] = xx [ 64 ] + xx [ 66 ] * xx [ 82 ] ;
pm_math_Quaternion_xform_ra ( xx + 72 , xx + 69 , xx + 92 ) ;
pm_math_Matrix3x3_transposeXform_ra ( xx + 154 , xx + 151 , xx + 69 ) ; xx [
64 ] = 0.1749999999976517 ; xx [ 98 ] = 2.825657975925348e-7 ; xx [ 99 ] = -
xx [ 98 ] ; xx [ 100 ] = 1.867941565314446e-13 ; xx [ 104 ] =
0.1410000000023632 ; xx [ 105 ] = 2.247598890146958e-8 ; xx [ 106 ] =
2.247598890146959e-8 ; xx [ 117 ] = 0.1749999999999851 ; xx [ 133 ] = xx [ 64
] ; xx [ 134 ] = xx [ 99 ] ; xx [ 135 ] = xx [ 100 ] ; xx [ 136 ] = xx [ 99 ]
; xx [ 137 ] = xx [ 104 ] ; xx [ 138 ] = xx [ 105 ] ; xx [ 139 ] = xx [ 100 ]
; xx [ 140 ] = xx [ 106 ] ; xx [ 141 ] = xx [ 117 ] ;
pm_math_Matrix3x3_xform_ra ( xx + 133 , xx + 54 , xx + 127 ) ;
pm_math_Matrix3x3_transposeXform_ra ( xx + 154 , xx + 164 , xx + 133 ) ; xx [
136 ] = xx [ 110 ] - ( xx [ 90 ] + xx [ 96 ] ) + xx [ 76 ] + xx [ 69 ] + xx [
127 ] + xx [ 66 ] * xx [ 133 ] ; xx [ 137 ] = xx [ 111 ] - ( xx [ 91 ] - xx [
97 ] ) + xx [ 77 ] + xx [ 70 ] + xx [ 128 ] + xx [ 66 ] * xx [ 134 ] ; xx [
138 ] = xx [ 112 ] - ( xx [ 83 ] + xx [ 84 ] ) + xx [ 78 ] + xx [ 71 ] + xx [
129 ] + xx [ 66 ] * xx [ 135 ] ; pm_math_Quaternion_xform_ra ( xx + 72 , xx +
136 , xx + 69 ) ; pm_math_Vector3_cross_ra ( xx + 145 , xx + 69 , xx + 76 ) ;
xx [ 83 ] = xx [ 72 ] * xx [ 72 ] ; xx [ 84 ] = 1.0 ; xx [ 90 ] = xx [ 73 ] *
xx [ 74 ] ; xx [ 91 ] = xx [ 72 ] * xx [ 75 ] ; xx [ 96 ] = xx [ 73 ] * xx [
75 ] ; xx [ 97 ] = xx [ 72 ] * xx [ 74 ] ; xx [ 99 ] = xx [ 74 ] * xx [ 75 ]
; xx [ 110 ] = xx [ 72 ] * xx [ 73 ] ; xx [ 154 ] = ( xx [ 83 ] + xx [ 73 ] *
xx [ 73 ] ) * xx [ 89 ] - xx [ 84 ] ; xx [ 155 ] = xx [ 89 ] * ( xx [ 90 ] -
xx [ 91 ] ) ; xx [ 156 ] = ( xx [ 96 ] + xx [ 97 ] ) * xx [ 89 ] ; xx [ 157 ]
= ( xx [ 90 ] + xx [ 91 ] ) * xx [ 89 ] ; xx [ 158 ] = ( xx [ 83 ] + xx [ 74
] * xx [ 74 ] ) * xx [ 89 ] - xx [ 84 ] ; xx [ 159 ] = xx [ 89 ] * ( xx [ 99
] - xx [ 110 ] ) ; xx [ 160 ] = xx [ 89 ] * ( xx [ 96 ] - xx [ 97 ] ) ; xx [
161 ] = ( xx [ 99 ] + xx [ 110 ] ) * xx [ 89 ] ; xx [ 162 ] = ( xx [ 83 ] +
xx [ 75 ] * xx [ 75 ] ) * xx [ 89 ] - xx [ 84 ] ; xx [ 83 ] = xx [ 80 ] / xx
[ 65 ] ; xx [ 90 ] = xx [ 83 ] * xx [ 81 ] ; xx [ 91 ] = xx [ 83 ] * xx [ 82
] ; xx [ 96 ] = xx [ 81 ] / xx [ 65 ] ; xx [ 97 ] = xx [ 96 ] * xx [ 82 ] ;
xx [ 99 ] = xx [ 82 ] / xx [ 65 ] ; xx [ 163 ] = xx [ 47 ] - xx [ 83 ] * xx [
80 ] ; xx [ 164 ] = xx [ 67 ] - xx [ 90 ] ; xx [ 165 ] = xx [ 102 ] - xx [ 91
] ; xx [ 166 ] = xx [ 103 ] - xx [ 90 ] ; xx [ 167 ] = xx [ 113 ] - xx [ 96 ]
* xx [ 81 ] ; xx [ 168 ] = - ( xx [ 95 ] + xx [ 97 ] ) ; xx [ 169 ] = xx [
114 ] - xx [ 91 ] ; xx [ 170 ] = - ( xx [ 101 ] + xx [ 97 ] ) ; xx [ 171 ] =
xx [ 115 ] - xx [ 99 ] * xx [ 82 ] ; pm_math_Matrix3x3_composeTranspose_ra (
xx + 163 , xx + 154 , xx + 172 ) ; pm_math_Matrix3x3_compose_ra ( xx + 154 ,
xx + 172 , xx + 163 ) ; xx [ 47 ] = xx [ 133 ] / xx [ 65 ] ; xx [ 67 ] = xx [
134 ] / xx [ 65 ] ; xx [ 90 ] = xx [ 135 ] / xx [ 65 ] ; xx [ 172 ] = - ( xx
[ 25 ] + xx [ 47 ] * xx [ 80 ] ) ; xx [ 173 ] = - ( xx [ 60 ] + xx [ 67 ] *
xx [ 80 ] ) ; xx [ 174 ] = - ( xx [ 68 ] + xx [ 90 ] * xx [ 80 ] ) ; xx [ 175
] = - ( xx [ 124 ] + xx [ 47 ] * xx [ 81 ] ) ; xx [ 176 ] = - ( xx [ 125 ] +
xx [ 67 ] * xx [ 81 ] ) ; xx [ 177 ] = - ( xx [ 126 ] + xx [ 90 ] * xx [ 81 ]
) ; xx [ 178 ] = - ( xx [ 142 ] + xx [ 47 ] * xx [ 82 ] ) ; xx [ 179 ] = - (
xx [ 143 ] + xx [ 67 ] * xx [ 82 ] ) ; xx [ 180 ] = - ( xx [ 144 ] + xx [ 90
] * xx [ 82 ] ) ; pm_math_Matrix3x3_composeTranspose_ra ( xx + 172 , xx + 154
, xx + 136 ) ; pm_math_Matrix3x3_compose_ra ( xx + 154 , xx + 136 , xx + 172
) ; pm_math_Matrix3x3_postCross_ra ( xx + 172 , xx + 145 , xx + 136 ) ; xx [
25 ] = - ( xx [ 98 ] + xx [ 47 ] * xx [ 134 ] ) ; xx [ 60 ] = xx [ 100 ] - xx
[ 47 ] * xx [ 135 ] ; xx [ 65 ] = xx [ 67 ] * xx [ 135 ] ; xx [ 181 ] = xx [
64 ] - xx [ 47 ] * xx [ 133 ] ; xx [ 182 ] = xx [ 25 ] ; xx [ 183 ] = xx [ 60
] ; xx [ 184 ] = xx [ 25 ] ; xx [ 185 ] = xx [ 104 ] - xx [ 67 ] * xx [ 134 ]
; xx [ 186 ] = xx [ 105 ] - xx [ 65 ] ; xx [ 187 ] = xx [ 60 ] ; xx [ 188 ] =
xx [ 106 ] - xx [ 65 ] ; xx [ 189 ] = xx [ 117 ] - xx [ 90 ] * xx [ 135 ] ;
pm_math_Matrix3x3_composeTranspose_ra ( xx + 181 , xx + 154 , xx + 190 ) ;
pm_math_Matrix3x3_compose_ra ( xx + 154 , xx + 190 , xx + 181 ) ;
pm_math_Matrix3x3_postCross_ra ( xx + 181 , xx + 145 , xx + 154 ) ;
pm_math_Matrix3x3_preCross_ra ( xx + 154 , xx + 145 , xx + 190 ) ; xx [ 25 ]
= xx [ 163 ] - xx [ 136 ] - xx [ 136 ] - xx [ 190 ] ; xx [ 60 ] = xx [ 164 ]
- xx [ 137 ] - xx [ 139 ] - xx [ 191 ] ; xx [ 64 ] = xx [ 165 ] - xx [ 138 ]
- xx [ 142 ] - xx [ 192 ] ; xx [ 65 ] = xx [ 166 ] - xx [ 139 ] - xx [ 137 ]
- xx [ 193 ] ; xx [ 68 ] = xx [ 167 ] - xx [ 140 ] - xx [ 140 ] - xx [ 194 ]
; xx [ 80 ] = xx [ 168 ] - xx [ 141 ] - xx [ 143 ] - xx [ 195 ] ; xx [ 81 ] =
xx [ 169 ] - xx [ 142 ] - xx [ 138 ] - xx [ 196 ] ; xx [ 82 ] = xx [ 170 ] -
xx [ 143 ] - xx [ 141 ] - xx [ 197 ] ; xx [ 91 ] = xx [ 171 ] - xx [ 144 ] -
xx [ 144 ] - xx [ 198 ] ; xx [ 133 ] = xx [ 48 ] + xx [ 25 ] ; xx [ 134 ] =
xx [ 60 ] ; xx [ 135 ] = xx [ 64 ] ; xx [ 136 ] = xx [ 65 ] ; xx [ 137 ] = xx
[ 49 ] + xx [ 68 ] ; xx [ 138 ] = xx [ 80 ] ; xx [ 139 ] = xx [ 81 ] ; xx [
140 ] = xx [ 82 ] ; xx [ 141 ] = xx [ 57 ] + xx [ 91 ] ; xx [ 100 ] = xx [ 19
] ; xx [ 101 ] = xx [ 38 ] ; xx [ 102 ] = xx [ 46 ] ;
pm_math_Vector3_cross_ra ( xx + 42 , xx + 100 , xx + 103 ) ;
pm_math_Matrix3x3_xform_ra ( xx + 133 , xx + 103 , xx + 42 ) ; xx [ 19 ] = xx
[ 172 ] - xx [ 154 ] ; xx [ 38 ] = xx [ 173 ] - xx [ 157 ] ; xx [ 46 ] = xx [
174 ] - xx [ 160 ] ; xx [ 95 ] = xx [ 175 ] - xx [ 155 ] ; xx [ 97 ] = xx [
176 ] - xx [ 158 ] ; xx [ 98 ] = xx [ 177 ] - xx [ 161 ] ; xx [ 100 ] = xx [
178 ] - xx [ 156 ] ; xx [ 101 ] = xx [ 179 ] - xx [ 159 ] ; xx [ 102 ] = xx [
180 ] - xx [ 162 ] ; xx [ 154 ] = xx [ 19 ] ; xx [ 155 ] = xx [ 38 ] ; xx [
156 ] = xx [ 46 ] ; xx [ 157 ] = xx [ 95 ] ; xx [ 158 ] = xx [ 97 ] ; xx [
159 ] = xx [ 98 ] ; xx [ 160 ] = xx [ 100 ] ; xx [ 161 ] = xx [ 101 ] ; xx [
162 ] = xx [ 102 ] ; xx [ 110 ] = 0.01017880053681747 ; xx [ 111 ] = -
3.736067231021393e-4 ; xx [ 112 ] = 0.1298316311137164 ;
pm_math_Vector3_cross_ra ( xx + 31 , xx + 110 , xx + 113 ) ;
pm_math_Vector3_cross_ra ( xx + 31 , xx + 113 , xx + 117 ) ;
pm_math_Quaternion_inverseXform_ra ( xx + 50 , xx + 117 , xx + 31 ) ;
pm_math_Matrix3x3_xform_ra ( xx + 154 , xx + 31 , xx + 113 ) ; xx [ 106 ] =
xx [ 61 ] + xx [ 92 ] + xx [ 76 ] + xx [ 42 ] + xx [ 113 ] ; xx [ 117 ] = xx
[ 23 ] ; xx [ 118 ] = xx [ 36 ] ; xx [ 119 ] = xx [ 37 ] ; xx [ 124 ] = xx [
62 ] + xx [ 93 ] + xx [ 77 ] + xx [ 43 ] + xx [ 114 ] ; xx [ 42 ] = xx [ 63 ]
+ xx [ 94 ] + xx [ 78 ] + xx [ 44 ] + xx [ 115 ] ; xx [ 61 ] = xx [ 106 ] ;
xx [ 62 ] = xx [ 124 ] ; xx [ 63 ] = xx [ 42 ] ; pm_math_Matrix3x3_xform_ra (
xx + 133 , xx + 117 , xx + 76 ) ; xx [ 43 ] = pm_math_Vector3_dot_ra ( xx +
117 , xx + 76 ) ; ii [ 0 ] = factorSymmetricPosDef ( xx + 43 , 1 , xx + 44 )
; if ( ii [ 0 ] != 0 ) { return sm_ssci_recordRunTimeError (
"sm:compiler:messages:simulationErrors:DegenerateMass" ,
 "'openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/joint3' has a degenerate mass distribution on its follower side."
, neDiagMgr ) ; } xx [ 44 ] = ( input [ 4 ] - xx [ 0 ] * state [ 5 ] -
pm_math_Vector3_dot_ra ( xx + 117 , xx + 61 ) ) / xx [ 43 ] ; xx [ 61 ] = xx
[ 106 ] + xx [ 44 ] * xx [ 76 ] ; xx [ 62 ] = xx [ 124 ] + xx [ 44 ] * xx [
77 ] ; xx [ 63 ] = xx [ 42 ] + xx [ 44 ] * xx [ 78 ] ;
pm_math_Quaternion_xform_ra ( xx + 50 , xx + 61 , xx + 92 ) ;
pm_math_Matrix3x3_transposeXform_ra ( xx + 154 , xx + 103 , xx + 61 ) ; xx [
42 ] = 0.131 ; xx [ 133 ] = xx [ 42 ] + xx [ 181 ] ; xx [ 134 ] = xx [ 182 ]
; xx [ 135 ] = xx [ 183 ] ; xx [ 136 ] = xx [ 184 ] ; xx [ 137 ] = xx [ 42 ]
+ xx [ 185 ] ; xx [ 138 ] = xx [ 186 ] ; xx [ 139 ] = xx [ 187 ] ; xx [ 140 ]
= xx [ 188 ] ; xx [ 141 ] = xx [ 42 ] + xx [ 189 ] ;
pm_math_Matrix3x3_xform_ra ( xx + 133 , xx + 31 , xx + 113 ) ;
pm_math_Matrix3x3_transposeXform_ra ( xx + 154 , xx + 117 , xx + 124 ) ; xx [
117 ] = xx [ 69 ] + xx [ 61 ] + xx [ 113 ] + xx [ 44 ] * xx [ 124 ] ; xx [
118 ] = xx [ 70 ] + xx [ 62 ] + xx [ 114 ] + xx [ 44 ] * xx [ 125 ] ; xx [
119 ] = xx [ 71 ] + xx [ 63 ] + xx [ 115 ] + xx [ 44 ] * xx [ 126 ] ;
pm_math_Quaternion_xform_ra ( xx + 50 , xx + 117 , xx + 61 ) ;
pm_math_Vector3_cross_ra ( xx + 110 , xx + 61 , xx + 69 ) ; xx [ 106 ] = xx [
50 ] * xx [ 50 ] ; xx [ 113 ] = xx [ 51 ] * xx [ 52 ] ; xx [ 114 ] = xx [ 50
] * xx [ 53 ] ; xx [ 115 ] = xx [ 51 ] * xx [ 53 ] ; xx [ 117 ] = xx [ 50 ] *
xx [ 52 ] ; xx [ 118 ] = xx [ 52 ] * xx [ 53 ] ; xx [ 119 ] = xx [ 50 ] * xx
[ 51 ] ; xx [ 133 ] = ( xx [ 106 ] + xx [ 51 ] * xx [ 51 ] ) * xx [ 89 ] - xx
[ 84 ] ; xx [ 134 ] = xx [ 89 ] * ( xx [ 113 ] - xx [ 114 ] ) ; xx [ 135 ] =
( xx [ 115 ] + xx [ 117 ] ) * xx [ 89 ] ; xx [ 136 ] = ( xx [ 113 ] + xx [
114 ] ) * xx [ 89 ] ; xx [ 137 ] = ( xx [ 106 ] + xx [ 52 ] * xx [ 52 ] ) *
xx [ 89 ] - xx [ 84 ] ; xx [ 138 ] = xx [ 89 ] * ( xx [ 118 ] - xx [ 119 ] )
; xx [ 139 ] = xx [ 89 ] * ( xx [ 115 ] - xx [ 117 ] ) ; xx [ 140 ] = ( xx [
118 ] + xx [ 119 ] ) * xx [ 89 ] ; xx [ 141 ] = ( xx [ 106 ] + xx [ 53 ] * xx
[ 53 ] ) * xx [ 89 ] - xx [ 84 ] ; xx [ 106 ] = xx [ 76 ] / xx [ 43 ] ; xx [
113 ] = xx [ 106 ] * xx [ 77 ] ; xx [ 114 ] = xx [ 106 ] * xx [ 78 ] ; xx [
115 ] = xx [ 77 ] / xx [ 43 ] ; xx [ 117 ] = xx [ 115 ] * xx [ 78 ] ; xx [
118 ] = xx [ 78 ] / xx [ 43 ] ; xx [ 154 ] = xx [ 25 ] - xx [ 106 ] * xx [ 76
] + xx [ 48 ] ; xx [ 155 ] = xx [ 60 ] - xx [ 113 ] ; xx [ 156 ] = xx [ 64 ]
- xx [ 114 ] ; xx [ 157 ] = xx [ 65 ] - xx [ 113 ] ; xx [ 158 ] = xx [ 68 ] -
xx [ 115 ] * xx [ 77 ] + xx [ 49 ] ; xx [ 159 ] = xx [ 80 ] - xx [ 117 ] ; xx
[ 160 ] = xx [ 81 ] - xx [ 114 ] ; xx [ 161 ] = xx [ 82 ] - xx [ 117 ] ; xx [
162 ] = xx [ 91 ] - xx [ 118 ] * xx [ 78 ] + xx [ 57 ] ;
pm_math_Matrix3x3_composeTranspose_ra ( xx + 154 , xx + 133 , xx + 163 ) ;
pm_math_Matrix3x3_compose_ra ( xx + 133 , xx + 163 , xx + 154 ) ; xx [ 25 ] =
xx [ 124 ] / xx [ 43 ] ; xx [ 48 ] = xx [ 125 ] / xx [ 43 ] ; xx [ 49 ] = xx
[ 126 ] / xx [ 43 ] ; xx [ 163 ] = xx [ 19 ] - xx [ 25 ] * xx [ 76 ] ; xx [
164 ] = xx [ 38 ] - xx [ 48 ] * xx [ 76 ] ; xx [ 165 ] = xx [ 46 ] - xx [ 49
] * xx [ 76 ] ; xx [ 166 ] = xx [ 95 ] - xx [ 25 ] * xx [ 77 ] ; xx [ 167 ] =
xx [ 97 ] - xx [ 48 ] * xx [ 77 ] ; xx [ 168 ] = xx [ 98 ] - xx [ 49 ] * xx [
77 ] ; xx [ 169 ] = xx [ 100 ] - xx [ 25 ] * xx [ 78 ] ; xx [ 170 ] = xx [
101 ] - xx [ 48 ] * xx [ 78 ] ; xx [ 171 ] = xx [ 102 ] - xx [ 49 ] * xx [ 78
] ; pm_math_Matrix3x3_composeTranspose_ra ( xx + 163 , xx + 133 , xx + 172 )
; pm_math_Matrix3x3_compose_ra ( xx + 133 , xx + 172 , xx + 163 ) ;
pm_math_Matrix3x3_postCross_ra ( xx + 163 , xx + 110 , xx + 172 ) ; xx [ 19 ]
= xx [ 25 ] * xx [ 125 ] ; xx [ 38 ] = xx [ 25 ] * xx [ 126 ] ; xx [ 43 ] =
xx [ 48 ] * xx [ 126 ] ; xx [ 190 ] = xx [ 181 ] - xx [ 25 ] * xx [ 124 ] +
xx [ 42 ] ; xx [ 191 ] = xx [ 182 ] - xx [ 19 ] ; xx [ 192 ] = xx [ 183 ] -
xx [ 38 ] ; xx [ 193 ] = xx [ 184 ] - xx [ 19 ] ; xx [ 194 ] = xx [ 185 ] -
xx [ 48 ] * xx [ 125 ] + xx [ 42 ] ; xx [ 195 ] = xx [ 186 ] - xx [ 43 ] ; xx
[ 196 ] = xx [ 187 ] - xx [ 38 ] ; xx [ 197 ] = xx [ 188 ] - xx [ 43 ] ; xx [
198 ] = xx [ 189 ] - xx [ 49 ] * xx [ 126 ] + xx [ 42 ] ;
pm_math_Matrix3x3_composeTranspose_ra ( xx + 190 , xx + 133 , xx + 181 ) ;
pm_math_Matrix3x3_compose_ra ( xx + 133 , xx + 181 , xx + 190 ) ;
pm_math_Matrix3x3_postCross_ra ( xx + 190 , xx + 110 , xx + 133 ) ;
pm_math_Matrix3x3_preCross_ra ( xx + 133 , xx + 110 , xx + 181 ) ; xx [ 19 ]
= xx [ 154 ] - xx [ 172 ] - xx [ 172 ] - xx [ 181 ] ; xx [ 38 ] = xx [ 155 ]
- xx [ 173 ] - xx [ 175 ] - xx [ 182 ] ; xx [ 42 ] = xx [ 156 ] - xx [ 174 ]
- xx [ 178 ] - xx [ 183 ] ; xx [ 43 ] = xx [ 157 ] - xx [ 175 ] - xx [ 173 ]
- xx [ 184 ] ; xx [ 46 ] = xx [ 158 ] - xx [ 176 ] - xx [ 176 ] - xx [ 185 ]
; xx [ 57 ] = xx [ 159 ] - xx [ 177 ] - xx [ 179 ] - xx [ 186 ] ; xx [ 60 ] =
xx [ 160 ] - xx [ 178 ] - xx [ 174 ] - xx [ 187 ] ; xx [ 64 ] = xx [ 161 ] -
xx [ 179 ] - xx [ 177 ] - xx [ 188 ] ; xx [ 65 ] = xx [ 162 ] - xx [ 180 ] -
xx [ 180 ] - xx [ 189 ] ; xx [ 154 ] = xx [ 26 ] + xx [ 19 ] ; xx [ 155 ] =
xx [ 38 ] ; xx [ 156 ] = xx [ 42 ] ; xx [ 157 ] = xx [ 43 ] ; xx [ 158 ] = xx
[ 34 ] + xx [ 46 ] ; xx [ 159 ] = xx [ 57 ] ; xx [ 160 ] = xx [ 60 ] ; xx [
161 ] = xx [ 64 ] ; xx [ 162 ] = xx [ 35 ] + xx [ 65 ] ; xx [ 76 ] = - xx [
11 ] ; xx [ 77 ] = xx [ 20 ] ; xx [ 78 ] = xx [ 24 ] ;
pm_math_Vector3_cross_ra ( xx + 16 , xx + 76 , xx + 80 ) ;
pm_math_Matrix3x3_xform_ra ( xx + 154 , xx + 80 , xx + 16 ) ; xx [ 11 ] = xx
[ 163 ] - xx [ 133 ] ; xx [ 20 ] = xx [ 164 ] - xx [ 136 ] ; xx [ 24 ] = xx [
165 ] - xx [ 139 ] ; xx [ 68 ] = xx [ 166 ] - xx [ 134 ] ; xx [ 76 ] = xx [
167 ] - xx [ 137 ] ; xx [ 77 ] = xx [ 168 ] - xx [ 140 ] ; xx [ 78 ] = xx [
169 ] - xx [ 135 ] ; xx [ 91 ] = xx [ 170 ] - xx [ 138 ] ; xx [ 95 ] = xx [
171 ] - xx [ 141 ] ; xx [ 133 ] = xx [ 11 ] ; xx [ 134 ] = xx [ 20 ] ; xx [
135 ] = xx [ 24 ] ; xx [ 136 ] = xx [ 68 ] ; xx [ 137 ] = xx [ 76 ] ; xx [
138 ] = xx [ 77 ] ; xx [ 139 ] = xx [ 78 ] ; xx [ 140 ] = xx [ 91 ] ; xx [
141 ] = xx [ 95 ] ; xx [ 100 ] = 2.543322406041548e-4 ; xx [ 101 ] = -
6.134092247471828e-4 ; xx [ 102 ] = 0.05799619853261406 ;
pm_math_Vector3_cross_ra ( xx + 7 , xx + 100 , xx + 124 ) ;
pm_math_Vector3_cross_ra ( xx + 7 , xx + 124 , xx + 127 ) ;
pm_math_Quaternion_inverseXform_ra ( xx + 27 , xx + 127 , xx + 7 ) ;
pm_math_Matrix3x3_xform_ra ( xx + 133 , xx + 7 , xx + 124 ) ; xx [ 97 ] = xx
[ 39 ] + xx [ 92 ] + xx [ 69 ] + xx [ 16 ] + xx [ 124 ] ; xx [ 127 ] = - xx [
12 ] ; xx [ 128 ] = xx [ 21 ] ; xx [ 129 ] = xx [ 22 ] ; xx [ 98 ] = xx [ 40
] + xx [ 93 ] + xx [ 70 ] + xx [ 17 ] + xx [ 125 ] ; xx [ 16 ] = xx [ 41 ] +
xx [ 94 ] + xx [ 71 ] + xx [ 18 ] + xx [ 126 ] ; xx [ 39 ] = xx [ 97 ] ; xx [
40 ] = xx [ 98 ] ; xx [ 41 ] = xx [ 16 ] ; pm_math_Matrix3x3_xform_ra ( xx +
154 , xx + 127 , xx + 69 ) ; xx [ 17 ] = pm_math_Vector3_dot_ra ( xx + 127 ,
xx + 69 ) ; ii [ 0 ] = factorSymmetricPosDef ( xx + 17 , 1 , xx + 18 ) ; if (
ii [ 0 ] != 0 ) { return sm_ssci_recordRunTimeError (
"sm:compiler:messages:simulationErrors:DegenerateMass" ,
 "'openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/joint2' has a degenerate mass distribution on its follower side."
, neDiagMgr ) ; } xx [ 18 ] = ( input [ 3 ] - xx [ 0 ] * state [ 3 ] -
pm_math_Vector3_dot_ra ( xx + 127 , xx + 39 ) ) / xx [ 17 ] ; xx [ 39 ] = xx
[ 97 ] + xx [ 18 ] * xx [ 69 ] ; xx [ 40 ] = xx [ 98 ] + xx [ 18 ] * xx [ 70
] ; xx [ 41 ] = xx [ 16 ] + xx [ 18 ] * xx [ 71 ] ;
pm_math_Quaternion_xform_ra ( xx + 27 , xx + 39 , xx + 92 ) ;
pm_math_Matrix3x3_transposeXform_ra ( xx + 133 , xx + 80 , xx + 39 ) ; xx [
16 ] = 0.136 ; xx [ 154 ] = xx [ 16 ] + xx [ 190 ] ; xx [ 155 ] = xx [ 191 ]
; xx [ 156 ] = xx [ 192 ] ; xx [ 157 ] = xx [ 193 ] ; xx [ 158 ] = xx [ 16 ]
+ xx [ 194 ] ; xx [ 159 ] = xx [ 195 ] ; xx [ 160 ] = xx [ 196 ] ; xx [ 161 ]
= xx [ 197 ] ; xx [ 162 ] = xx [ 16 ] + xx [ 198 ] ;
pm_math_Matrix3x3_xform_ra ( xx + 154 , xx + 7 , xx + 124 ) ;
pm_math_Matrix3x3_transposeXform_ra ( xx + 133 , xx + 127 , xx + 142 ) ; xx [
127 ] = xx [ 61 ] + xx [ 39 ] + xx [ 124 ] + xx [ 18 ] * xx [ 142 ] ; xx [
128 ] = xx [ 62 ] + xx [ 40 ] + xx [ 125 ] + xx [ 18 ] * xx [ 143 ] ; xx [
129 ] = xx [ 63 ] + xx [ 41 ] + xx [ 126 ] + xx [ 18 ] * xx [ 144 ] ;
pm_math_Quaternion_xform_ra ( xx + 27 , xx + 127 , xx + 39 ) ;
pm_math_Vector3_cross_ra ( xx + 100 , xx + 39 , xx + 61 ) ; xx [ 39 ] = xx [
13 ] + xx [ 92 ] + xx [ 61 ] ; xx [ 40 ] = xx [ 14 ] + xx [ 93 ] + xx [ 62 ]
; xx [ 41 ] = xx [ 15 ] + xx [ 94 ] + xx [ 63 ] ; xx [ 13 ] = xx [ 27 ] * xx
[ 27 ] ; xx [ 14 ] = xx [ 28 ] * xx [ 29 ] ; xx [ 15 ] = xx [ 27 ] * xx [ 30
] ; xx [ 61 ] = xx [ 28 ] * xx [ 30 ] ; xx [ 62 ] = xx [ 27 ] * xx [ 29 ] ;
xx [ 63 ] = xx [ 29 ] * xx [ 30 ] ; xx [ 92 ] = xx [ 27 ] * xx [ 28 ] ; xx [
133 ] = ( xx [ 13 ] + xx [ 28 ] * xx [ 28 ] ) * xx [ 89 ] - xx [ 84 ] ; xx [
134 ] = xx [ 89 ] * ( xx [ 14 ] - xx [ 15 ] ) ; xx [ 135 ] = ( xx [ 61 ] + xx
[ 62 ] ) * xx [ 89 ] ; xx [ 136 ] = ( xx [ 14 ] + xx [ 15 ] ) * xx [ 89 ] ;
xx [ 137 ] = ( xx [ 13 ] + xx [ 29 ] * xx [ 29 ] ) * xx [ 89 ] - xx [ 84 ] ;
xx [ 138 ] = xx [ 89 ] * ( xx [ 63 ] - xx [ 92 ] ) ; xx [ 139 ] = xx [ 89 ] *
( xx [ 61 ] - xx [ 62 ] ) ; xx [ 140 ] = ( xx [ 63 ] + xx [ 92 ] ) * xx [ 89
] ; xx [ 141 ] = ( xx [ 13 ] + xx [ 30 ] * xx [ 30 ] ) * xx [ 89 ] - xx [ 84
] ; xx [ 13 ] = xx [ 69 ] / xx [ 17 ] ; xx [ 14 ] = xx [ 13 ] * xx [ 70 ] ;
xx [ 15 ] = xx [ 13 ] * xx [ 71 ] ; xx [ 61 ] = xx [ 70 ] / xx [ 17 ] ; xx [
62 ] = xx [ 61 ] * xx [ 71 ] ; xx [ 63 ] = xx [ 71 ] / xx [ 17 ] ; xx [ 154 ]
= xx [ 19 ] - xx [ 13 ] * xx [ 69 ] + xx [ 26 ] ; xx [ 155 ] = xx [ 38 ] - xx
[ 14 ] ; xx [ 156 ] = xx [ 42 ] - xx [ 15 ] ; xx [ 157 ] = xx [ 43 ] - xx [
14 ] ; xx [ 158 ] = xx [ 46 ] - xx [ 61 ] * xx [ 70 ] + xx [ 34 ] ; xx [ 159
] = xx [ 57 ] - xx [ 62 ] ; xx [ 160 ] = xx [ 60 ] - xx [ 15 ] ; xx [ 161 ] =
xx [ 64 ] - xx [ 62 ] ; xx [ 162 ] = xx [ 65 ] - xx [ 63 ] * xx [ 71 ] + xx [
35 ] ; pm_math_Matrix3x3_composeTranspose_ra ( xx + 154 , xx + 133 , xx + 163
) ; pm_math_Matrix3x3_compose_ra ( xx + 133 , xx + 163 , xx + 154 ) ; xx [ 14
] = xx [ 142 ] / xx [ 17 ] ; xx [ 15 ] = xx [ 143 ] / xx [ 17 ] ; xx [ 19 ] =
xx [ 144 ] / xx [ 17 ] ; xx [ 163 ] = xx [ 11 ] - xx [ 14 ] * xx [ 69 ] ; xx
[ 164 ] = xx [ 20 ] - xx [ 15 ] * xx [ 69 ] ; xx [ 165 ] = xx [ 24 ] - xx [
19 ] * xx [ 69 ] ; xx [ 166 ] = xx [ 68 ] - xx [ 14 ] * xx [ 70 ] ; xx [ 167
] = xx [ 76 ] - xx [ 15 ] * xx [ 70 ] ; xx [ 168 ] = xx [ 77 ] - xx [ 19 ] *
xx [ 70 ] ; xx [ 169 ] = xx [ 78 ] - xx [ 14 ] * xx [ 71 ] ; xx [ 170 ] = xx
[ 91 ] - xx [ 15 ] * xx [ 71 ] ; xx [ 171 ] = xx [ 95 ] - xx [ 19 ] * xx [ 71
] ; pm_math_Matrix3x3_composeTranspose_ra ( xx + 163 , xx + 133 , xx + 172 )
; pm_math_Matrix3x3_compose_ra ( xx + 133 , xx + 172 , xx + 163 ) ;
pm_math_Matrix3x3_postCross_ra ( xx + 163 , xx + 100 , xx + 172 ) ; xx [ 11 ]
= xx [ 14 ] * xx [ 143 ] ; xx [ 17 ] = xx [ 14 ] * xx [ 144 ] ; xx [ 20 ] =
xx [ 15 ] * xx [ 144 ] ; xx [ 181 ] = xx [ 190 ] - xx [ 14 ] * xx [ 142 ] +
xx [ 16 ] ; xx [ 182 ] = xx [ 191 ] - xx [ 11 ] ; xx [ 183 ] = xx [ 192 ] -
xx [ 17 ] ; xx [ 184 ] = xx [ 193 ] - xx [ 11 ] ; xx [ 185 ] = xx [ 194 ] -
xx [ 15 ] * xx [ 143 ] + xx [ 16 ] ; xx [ 186 ] = xx [ 195 ] - xx [ 20 ] ; xx
[ 187 ] = xx [ 196 ] - xx [ 17 ] ; xx [ 188 ] = xx [ 197 ] - xx [ 20 ] ; xx [
189 ] = xx [ 198 ] - xx [ 19 ] * xx [ 144 ] + xx [ 16 ] ;
pm_math_Matrix3x3_composeTranspose_ra ( xx + 181 , xx + 133 , xx + 190 ) ;
pm_math_Matrix3x3_compose_ra ( xx + 133 , xx + 190 , xx + 181 ) ;
pm_math_Matrix3x3_postCross_ra ( xx + 181 , xx + 100 , xx + 133 ) ;
pm_math_Matrix3x3_preCross_ra ( xx + 133 , xx + 100 , xx + 181 ) ; xx [ 190 ]
= 2.559811676117321e-4 + xx [ 154 ] - xx [ 172 ] - xx [ 172 ] - xx [ 181 ] ;
xx [ 191 ] = xx [ 155 ] - xx [ 173 ] - xx [ 175 ] - xx [ 182 ] ; xx [ 192 ] =
xx [ 156 ] - xx [ 174 ] - xx [ 178 ] - xx [ 183 ] ; xx [ 193 ] = xx [ 157 ] -
xx [ 175 ] - xx [ 173 ] - xx [ 184 ] ; xx [ 194 ] = 2.541291830521279e-4 + xx
[ 158 ] - xx [ 176 ] - xx [ 176 ] - xx [ 185 ] ; xx [ 195 ] = xx [ 159 ] - xx
[ 177 ] - xx [ 179 ] - xx [ 186 ] ; xx [ 196 ] = xx [ 160 ] - xx [ 178 ] - xx
[ 174 ] - xx [ 187 ] ; xx [ 197 ] = xx [ 161 ] - xx [ 179 ] - xx [ 177 ] - xx
[ 188 ] ; xx [ 198 ] = 1.885715273225301e-5 + xx [ 162 ] - xx [ 180 ] - xx [
180 ] - xx [ 189 ] ; pm_math_Matrix3x3_xform_ra ( xx + 190 , xx + 4 , xx + 68
) ; xx [ 11 ] = pm_math_Vector3_dot_ra ( xx + 4 , xx + 68 ) ; ii [ 0 ] =
factorSymmetricPosDef ( xx + 11 , 1 , xx + 16 ) ; if ( ii [ 0 ] != 0 ) {
return sm_ssci_recordRunTimeError (
"sm:compiler:messages:simulationErrors:DegenerateMass" ,
 "'openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/joint1' has a degenerate mass distribution on its follower side."
, neDiagMgr ) ; } xx [ 154 ] = xx [ 163 ] - xx [ 133 ] ; xx [ 155 ] = xx [
164 ] - xx [ 136 ] ; xx [ 156 ] = xx [ 165 ] - xx [ 139 ] ; xx [ 157 ] = xx [
166 ] - xx [ 134 ] ; xx [ 158 ] = xx [ 167 ] - xx [ 137 ] ; xx [ 159 ] = xx [
168 ] - xx [ 140 ] ; xx [ 160 ] = xx [ 169 ] - xx [ 135 ] ; xx [ 161 ] = xx [
170 ] - xx [ 138 ] ; xx [ 162 ] = xx [ 171 ] - xx [ 141 ] ;
pm_math_Matrix3x3_transposeXform_ra ( xx + 154 , xx + 4 , xx + 68 ) ; xx [ 76
] = xx [ 68 ] / xx [ 11 ] ; xx [ 77 ] = xx [ 69 ] / xx [ 11 ] ; xx [ 78 ] =
xx [ 70 ] / xx [ 11 ] ; xx [ 68 ] = - 0.9999793598170716 ; xx [ 69 ] =
5.294470415447712e-3 ; xx [ 70 ] = 2.177120480925496e-3 ; xx [ 71 ] =
2.916962336305661e-3 ; xx [ 16 ] = xx [ 10 ] * state [ 0 ] ; xx [ 10 ] = sin
( xx [ 16 ] ) ; xx [ 91 ] = cos ( xx [ 16 ] ) ; xx [ 92 ] = xx [ 1 ] * xx [
10 ] ; xx [ 93 ] = - ( xx [ 2 ] * xx [ 10 ] ) ; xx [ 94 ] = xx [ 3 ] * xx [
10 ] ; pm_math_Quaternion_compose_ra ( xx + 68 , xx + 91 , xx + 124 ) ; xx [
10 ] = 9.806649999999999 ; xx [ 16 ] = xx [ 10 ] * xx [ 125 ] ; xx [ 17 ] =
xx [ 10 ] * xx [ 126 ] ; xx [ 20 ] = xx [ 89 ] * ( xx [ 127 ] * xx [ 16 ] -
xx [ 124 ] * xx [ 17 ] ) ; xx [ 24 ] = ( xx [ 124 ] * xx [ 16 ] + xx [ 127 ]
* xx [ 17 ] ) * xx [ 89 ] ; xx [ 26 ] = ( xx [ 125 ] * xx [ 16 ] + xx [ 126 ]
* xx [ 17 ] ) * xx [ 89 ] ; xx [ 68 ] = xx [ 20 ] ; xx [ 69 ] = xx [ 24 ] ;
xx [ 70 ] = xx [ 10 ] - xx [ 26 ] ; xx [ 16 ] = ( input [ 2 ] - xx [ 0 ] *
state [ 1 ] - pm_math_Vector3_dot_ra ( xx + 4 , xx + 39 ) ) / xx [ 11 ] -
pm_math_Vector3_dot_ra ( xx + 76 , xx + 68 ) ; xx [ 4 ] = xx [ 13 ] ; xx [ 5
] = xx [ 61 ] ; xx [ 6 ] = xx [ 63 ] ; xx [ 38 ] = xx [ 1 ] * xx [ 16 ] ; xx
[ 39 ] = - ( xx [ 2 ] * xx [ 16 ] ) ; xx [ 40 ] = xx [ 3 ] * xx [ 16 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 27 , xx + 38 , xx + 0 ) ; xx [ 41 ]
= xx [ 14 ] ; xx [ 42 ] = xx [ 15 ] ; xx [ 43 ] = xx [ 19 ] ;
pm_math_Vector3_cross_ra ( xx + 38 , xx + 100 , xx + 13 ) ; xx [ 38 ] = xx [
20 ] + xx [ 13 ] ; xx [ 39 ] = xx [ 24 ] + xx [ 14 ] ; xx [ 40 ] = xx [ 15 ]
- xx [ 26 ] + xx [ 10 ] ; pm_math_Quaternion_inverseXform_ra ( xx + 27 , xx +
38 , xx + 13 ) ; xx [ 3 ] = xx [ 18 ] - ( pm_math_Vector3_dot_ra ( xx + 4 ,
xx + 0 ) + pm_math_Vector3_dot_ra ( xx + 41 , xx + 13 ) ) ; xx [ 4 ] = xx [
106 ] ; xx [ 5 ] = xx [ 115 ] ; xx [ 6 ] = xx [ 118 ] ; xx [ 17 ] = xx [ 0 ]
- xx [ 12 ] * xx [ 3 ] + xx [ 80 ] ; xx [ 18 ] = xx [ 1 ] + xx [ 21 ] * xx [
3 ] + xx [ 81 ] ; xx [ 19 ] = xx [ 2 ] + xx [ 22 ] * xx [ 3 ] + xx [ 82 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 50 , xx + 17 , xx + 0 ) ; xx [ 10 ]
= xx [ 25 ] ; xx [ 11 ] = xx [ 48 ] ; xx [ 12 ] = xx [ 49 ] ;
pm_math_Vector3_cross_ra ( xx + 17 , xx + 110 , xx + 20 ) ; xx [ 17 ] = xx [
13 ] + xx [ 7 ] + xx [ 20 ] ; xx [ 18 ] = xx [ 14 ] + xx [ 8 ] + xx [ 21 ] ;
xx [ 19 ] = xx [ 15 ] + xx [ 9 ] + xx [ 22 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 50 , xx + 17 , xx + 7 ) ; xx [ 13 ]
= xx [ 44 ] - ( pm_math_Vector3_dot_ra ( xx + 4 , xx + 0 ) +
pm_math_Vector3_dot_ra ( xx + 10 , xx + 7 ) ) ; xx [ 4 ] = xx [ 83 ] ; xx [ 5
] = xx [ 96 ] ; xx [ 6 ] = xx [ 99 ] ; xx [ 10 ] = xx [ 0 ] + xx [ 23 ] * xx
[ 13 ] + xx [ 103 ] ; xx [ 11 ] = xx [ 1 ] + xx [ 36 ] * xx [ 13 ] + xx [ 104
] ; xx [ 12 ] = xx [ 2 ] + xx [ 37 ] * xx [ 13 ] + xx [ 105 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 72 , xx + 10 , xx + 0 ) ; xx [ 17 ]
= xx [ 47 ] ; xx [ 18 ] = xx [ 67 ] ; xx [ 19 ] = xx [ 90 ] ;
pm_math_Vector3_cross_ra ( xx + 10 , xx + 145 , xx + 20 ) ; xx [ 10 ] = xx [
7 ] + xx [ 31 ] + xx [ 20 ] ; xx [ 11 ] = xx [ 8 ] + xx [ 32 ] + xx [ 21 ] ;
xx [ 12 ] = xx [ 9 ] + xx [ 33 ] + xx [ 22 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 72 , xx + 10 , xx + 7 ) ; xx [ 10 ]
= xx [ 66 ] - ( pm_math_Vector3_dot_ra ( xx + 4 , xx + 0 ) +
pm_math_Vector3_dot_ra ( xx + 17 , xx + 7 ) ) ; xx [ 4 ] = xx [ 7 ] + xx [ 54
] ; xx [ 5 ] = xx [ 0 ] + xx [ 45 ] * xx [ 10 ] + xx [ 151 ] ; xx [ 6 ] = xx
[ 1 ] + xx [ 58 ] * xx [ 10 ] + xx [ 152 ] ; xx [ 7 ] = xx [ 2 ] - xx [ 59 ]
* xx [ 10 ] + xx [ 153 ] ; pm_math_Vector3_cross_ra ( xx + 5 , xx + 130 , xx
+ 0 ) ; xx [ 11 ] = xx [ 8 ] + xx [ 55 ] ; xx [ 8 ] = xx [ 9 ] + xx [ 56 ] ;
xx [ 17 ] = xx [ 4 ] + xx [ 0 ] ; xx [ 18 ] = xx [ 11 ] + xx [ 1 ] ; xx [ 19
] = xx [ 8 ] + xx [ 2 ] ; pm_math_Quaternion_inverseXform_ra ( xx + 120 , xx
+ 17 , xx + 0 ) ; pm_math_Vector3_cross_ra ( xx + 5 , xx + 107 , xx + 17 ) ;
xx [ 5 ] = xx [ 4 ] + xx [ 17 ] ; xx [ 6 ] = xx [ 11 ] + xx [ 18 ] ; xx [ 7 ]
= xx [ 8 ] + xx [ 19 ] ; pm_math_Quaternion_inverseXform_ra ( xx + 85 , xx +
5 , xx + 17 ) ; deriv [ 0 ] = state [ 1 ] ; deriv [ 1 ] = xx [ 16 ] ; deriv [
2 ] = state [ 3 ] ; deriv [ 3 ] = xx [ 3 ] ; deriv [ 4 ] = state [ 5 ] ;
deriv [ 5 ] = xx [ 13 ] ; deriv [ 6 ] = state [ 7 ] ; deriv [ 7 ] = xx [ 10 ]
; deriv [ 8 ] = state [ 9 ] ; deriv [ 9 ] = xx [ 79 ] + xx [ 1 ] ; deriv [ 10
] = state [ 11 ] ; deriv [ 11 ] = xx [ 116 ] - xx [ 18 ] ; errorResult [ 0 ]
= 0.0 ; return NULL ; } PmfMessageId
openManipulatorTrajectoryPlanning_6a081443_1_numJacPerturbLoBounds ( const
RuntimeDerivedValuesBundle * rtdv , const int * eqnEnableFlags , const double
* state , const int * modeVector , const double * input , const double *
inputDot , const double * inputDdot , const double * discreteState , double *
bounds , double * errorResult , NeuDiagnosticManager * neDiagMgr ) { const
double * rtdvd = rtdv -> mDoubles . mValues ; const int * rtdvi = rtdv ->
mInts . mValues ; double xx [ 2 ] ; ( void ) rtdvd ; ( void ) rtdvi ; ( void
) eqnEnableFlags ; ( void ) state ; ( void ) modeVector ; ( void ) input ; (
void ) inputDot ; ( void ) inputDdot ; ( void ) discreteState ; ( void )
neDiagMgr ; xx [ 0 ] = 1.0e-8 ; xx [ 1 ] = 1.0e-9 ; bounds [ 0 ] = xx [ 0 ] ;
bounds [ 1 ] = xx [ 0 ] ; bounds [ 2 ] = xx [ 0 ] ; bounds [ 3 ] = xx [ 0 ] ;
bounds [ 4 ] = xx [ 0 ] ; bounds [ 5 ] = xx [ 0 ] ; bounds [ 6 ] = xx [ 0 ] ;
bounds [ 7 ] = xx [ 0 ] ; bounds [ 8 ] = xx [ 1 ] ; bounds [ 9 ] = xx [ 1 ] ;
bounds [ 10 ] = xx [ 1 ] ; bounds [ 11 ] = xx [ 1 ] ; errorResult [ 0 ] = 0.0
; return NULL ; } PmfMessageId
openManipulatorTrajectoryPlanning_6a081443_1_numJacPerturbHiBounds ( const
RuntimeDerivedValuesBundle * rtdv , const int * eqnEnableFlags , const double
* state , const int * modeVector , const double * input , const double *
inputDot , const double * inputDdot , const double * discreteState , double *
bounds , double * errorResult , NeuDiagnosticManager * neDiagMgr ) { const
double * rtdvd = rtdv -> mDoubles . mValues ; const int * rtdvi = rtdv ->
mInts . mValues ; double xx [ 2 ] ; ( void ) rtdvd ; ( void ) rtdvi ; ( void
) eqnEnableFlags ; ( void ) state ; ( void ) modeVector ; ( void ) input ; (
void ) inputDot ; ( void ) inputDdot ; ( void ) discreteState ; ( void )
neDiagMgr ; xx [ 0 ] = 1.0 ; xx [ 1 ] = + pmf_get_inf ( ) ; bounds [ 0 ] = xx
[ 0 ] ; bounds [ 1 ] = xx [ 1 ] ; bounds [ 2 ] = xx [ 0 ] ; bounds [ 3 ] = xx
[ 1 ] ; bounds [ 4 ] = xx [ 0 ] ; bounds [ 5 ] = xx [ 1 ] ; bounds [ 6 ] = xx
[ 0 ] ; bounds [ 7 ] = xx [ 1 ] ; bounds [ 8 ] = xx [ 1 ] ; bounds [ 9 ] = xx
[ 1 ] ; bounds [ 10 ] = xx [ 1 ] ; bounds [ 11 ] = xx [ 1 ] ; errorResult [ 0
] = 0.0 ; return NULL ; }
